#!/usr/bin/env python3
"""
Velocity Controller - 速度控制器
负责接收导航系统的速度指令，并通过串口发送给无人机
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String, Float32MultiArray
import json
import time
import threading

# 注意：串口功能暂时注释，需要时取消注释
# import serial

class VelocityController(Node):
    """
    速度控制器
    功能：
    1. 接收/cmd_vel速度指令
    2. 处理和限制速度指令
    3. 通过串口发送给无人机（预留接口）
    4. 发布控制状态和调试信息
    """
    
    def __init__(self):
        super().__init__('velocity_controller')
        
        # 控制参数
        self.declare_parameter('max_linear_velocity', 2.0)  # 最大线速度 m/s
        self.declare_parameter('max_angular_velocity', 1.0) # 最大角速度 rad/s
        self.declare_parameter('velocity_deadband', 0.01)   # 速度死区
        self.declare_parameter('control_frequency', 20.0)   # 控制频率 Hz
        
        # 串口参数（预留）
        self.declare_parameter('serial_port', '/dev/ttyUSB0')  # 串口设备
        self.declare_parameter('serial_baudrate', 115200)      # 波特率
        self.declare_parameter('enable_serial', False)        # 是否启用串口
        
        self.max_linear_vel = self.get_parameter('max_linear_velocity').value
        self.max_angular_vel = self.get_parameter('max_angular_velocity').value
        self.velocity_deadband = self.get_parameter('velocity_deadband').value
        self.control_freq = self.get_parameter('control_frequency').value
        
        self.serial_port = self.get_parameter('serial_port').value
        self.serial_baudrate = self.get_parameter('serial_baudrate').value
        self.enable_serial = self.get_parameter('enable_serial').value
        
        # 状态变量
        self.current_cmd_vel = Twist()
        self.last_cmd_time = time.time()
        self.serial_connection = None
        self.command_count = 0
        self.error_count = 0
        
        # 安全相关
        self.emergency_stop = False
        self.max_idle_time = 1.0  # 最大空闲时间（秒）
        
        # 发布器
        self.status_pub = self.create_publisher(
            String, '/velocity_status', 10)
        self.debug_pub = self.create_publisher(
            Float32MultiArray, '/velocity_debug', 10)
        self.filtered_cmd_pub = self.create_publisher(
            Twist, '/cmd_vel_filtered', 10)
            
        # 订阅器
        self.cmd_vel_sub = self.create_subscription(
            Twist, '/cmd_vel', self.cmd_vel_callback, 10)
        self.emergency_sub = self.create_subscription(
            String, '/emergency_stop', self.emergency_callback, 10)
            
        # 初始化串口（如果启用）
        if self.enable_serial:
            self.init_serial()
            
        # 创建控制定时器
        self.control_timer = self.create_timer(
            1.0 / self.control_freq, self.control_loop)
            
        # 创建安全检查定时器
        self.safety_timer = self.create_timer(
            0.5, self.safety_check)
            
        self.get_logger().info(f"速度控制器已启动")
        self.get_logger().info(f"串口: {'启用' if self.enable_serial else '禁用'}")
        self.get_logger().info(f"最大线速度: {self.max_linear_vel} m/s")
        self.get_logger().info(f"最大角速度: {self.max_angular_vel} rad/s")
        
    def init_serial(self):
        """初始化串口连接"""
        try:
            # 注意：实际使用时需要安装pyserial: pip install pyserial
            # import serial
            # self.serial_connection = serial.Serial(
            #     port=self.serial_port,
            #     baudrate=self.serial_baudrate,
            #     timeout=1.0
            # )
            # self.get_logger().info(f"串口连接成功: {self.serial_port}")
            
            # 临时：模拟串口连接
            self.serial_connection = "模拟串口连接"
            self.get_logger().info(f"模拟串口连接: {self.serial_port}")
            
        except Exception as e:
            self.get_logger().error(f"串口连接失败: {str(e)}")
            self.serial_connection = None
            
    def cmd_vel_callback(self, msg):
        """速度指令回调"""
        if self.emergency_stop:
            self.get_logger().warn("紧急停止状态，忽略速度指令")
            return
            
        # 更新接收时间
        self.last_cmd_time = time.time()
        
        # 处理和限制速度指令
        filtered_cmd = self.filter_velocity_command(msg)
        
        # 保存当前指令
        self.current_cmd_vel = filtered_cmd
        
        # 发布过滤后的指令（用于调试）
        self.filtered_cmd_pub.publish(filtered_cmd)
        
        # 统计
        self.command_count += 1
        
    def emergency_callback(self, msg):
        """紧急停止回调"""
        if msg.data.lower() in ['stop', 'emergency', '停止', '紧急']:
            self.emergency_stop = True
            self.get_logger().warn("收到紧急停止指令！")
            # 立即发送停止指令
            self.send_stop_command()
        elif msg.data.lower() in ['resume', 'continue', '恢复', '继续']:
            self.emergency_stop = False
            self.get_logger().info("恢复正常运行")
            
    def filter_velocity_command(self, cmd_vel):
        """过滤和限制速度指令 - 优化版本"""
        filtered_cmd = Twist()
        
        # 向量化限制操作
        linear_limits = [self.max_linear_vel, self.max_linear_vel, self.max_linear_vel * 0.5]
        linear_values = [cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.linear.z]
        
        # 批量限制线速度
        for i, (val, limit) in enumerate(zip(linear_values, linear_limits)):
            limited_val = max(-limit, min(limit, val))
            # 应用死区
            if abs(limited_val) > self.velocity_deadband:
                if i == 0:
                    filtered_cmd.linear.x = limited_val
                elif i == 1:
                    filtered_cmd.linear.y = limited_val
                else:
                    filtered_cmd.linear.z = limited_val
        
        # 角速度处理（只需要Z轴）
        angular_z = max(-self.max_angular_vel, min(self.max_angular_vel, cmd_vel.angular.z))
        if abs(angular_z) > self.velocity_deadband:
            filtered_cmd.angular.z = angular_z
        
        return filtered_cmd
        
    def control_loop(self):
        """主控制循环 - 优化版本"""
        if self.emergency_stop:
            self.send_stop_command()
            return
            
        # 发送当前速度指令
        if self.current_cmd_vel is not None:
            self.send_velocity_command(self.current_cmd_vel)
            
        # 减少调试信息发布频率
        if self.command_count % 10 == 0:  # 每10次循环发布一次调试信息
            self.publish_debug_info()
            
    def safety_check(self):
        """安全检查 - 优化版本"""
        current_time = time.time()
        
        # 检查指令超时
        if current_time - self.last_cmd_time > self.max_idle_time:
            if not self.emergency_stop:
                self.get_logger().warn("速度指令超时，执行安全停止")
                self.send_stop_command()
                
        # 定期发布状态（降低频率）
        if self.command_count % 20 == 0:  # 每20次循环发布一次状态
            self.publish_status()
            
    def send_velocity_command(self, cmd_vel):
        """发送速度指令 - 优化版本"""
        try:
            # 构建指令字典（减少字符串格式化开销）
            cmd_dict = {
                'type': 'velocity',
                'linear': {
                    'x': round(cmd_vel.linear.x, 3),
                    'y': round(cmd_vel.linear.y, 3), 
                    'z': round(cmd_vel.linear.z, 3)
                },
                'angular': {
                    'z': round(cmd_vel.angular.z, 3)
                },
                'timestamp': time.time()
            }
            
            # 转换为JSON（只在需要时）
            if self.enable_serial:
                json_str = json.dumps(cmd_dict, separators=(',', ':'))  # 紧凑格式
                self.send_to_serial(json_str)
            
            # 更新统计
            self.command_count += 1
            
        except Exception as e:
            self.error_count += 1
            if self.error_count % 10 == 0:  # 减少错误日志频率
                self.get_logger().error(f"发送速度指令失败: {str(e)}")
                
    def send_stop_command(self):
        """发送停止指令"""
        stop_cmd = Twist()  # 所有速度为0
        self.send_velocity_command(stop_cmd)
        
    def send_to_serial(self, data):
        """通过串口发送数据"""
        try:
            if self.serial_connection:
                # 实际串口发送（需要时取消注释）
                # message = data + '\n'
                # self.serial_connection.write(message.encode('utf-8'))
                
                # 模拟发送
                if self.command_count % 100 == 0:  # 降低打印频率
                    self.get_logger().debug(f"模拟串口发送: {data}")
                    
        except Exception as e:
            self.error_count += 1
            self.get_logger().error(f"串口发送失败: {str(e)}")
            
    def publish_status(self):
        """发布控制器状态"""
        status_data = {
            'emergency_stop': self.emergency_stop,
            'serial_connected': self.serial_connection is not None,
            'command_count': self.command_count,
            'error_count': self.error_count,
            'last_cmd_age': time.time() - self.last_cmd_time
        }
        
        status_msg = String()
        status_msg.data = json.dumps(status_data)
        self.status_pub.publish(status_msg)
        
    def publish_debug_info(self):
        """发布调试信息 - 优化版本"""
        debug_array = Float32MultiArray()
        
        # 使用预分配的数组
        debug_data = [
            float(self.current_cmd_vel.linear.x) if self.current_cmd_vel else 0.0,
            float(self.current_cmd_vel.linear.y) if self.current_cmd_vel else 0.0,
            float(self.current_cmd_vel.linear.z) if self.current_cmd_vel else 0.0,
            float(self.current_cmd_vel.angular.z) if self.current_cmd_vel else 0.0,
            float(self.command_count),
            float(self.error_count),
            float(time.time() - self.last_cmd_time)
        ]
        
        debug_array.data = debug_data
        self.debug_pub.publish(debug_array)
        
    def __del__(self):
        """析构函数，关闭串口连接"""
        if self.serial_connection:
            try:
                # 实际串口关闭（需要时取消注释）
                # self.serial_connection.close()
                pass
            except:
                pass

def main(args=None):
    rclpy.init(args=args)
    
    velocity_controller = VelocityController()
    
    try:
        rclpy.spin(velocity_controller)
    except KeyboardInterrupt:
        pass
    finally:
        velocity_controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main() 